//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcRadarTask.h>
#include <iostream>
#include <vector>
#include <base/slog.hpp>
#include <rc_message/rc_msg_server.h>
#include <boost/thread/mutex.hpp>
#include <rc_mapping/slam_devices.h>


namespace RC {
    namespace Task {

        void radar_call_function(std::vector <RC::Map::DOT> dots) {
//            for (int i = 0; i < dots.size(); i++) {
//                std::cout << "dist: " << dots[i].dist << " theta: " << dots[i].theta << std::endl;
//            }
            RC::Message::MessageServer::radarMessage.push_message(dots);
        }
        void run_radar_task(rc_RadarInfo rcRadarInfo){
            using namespace RC::Message;
            std::string radar_path=rcRadarInfo.radar_device;
            slog::info << "雷达任务启动中->" <<radar_path<< slog::endl;

            SlamDevice slamDevice(radar_path);
            slamDevice.bind(radar_call_function);
            slamDevice.start(0, 1);
            // 读取雷达数据
            try {
                while (true) {
                    boost::this_thread::interruption_point();
                    boost::mutex::scoped_lock lock(RadarMessage::radar_mutex);
                    MessageServer::radarMessage.pop_message();
                    slamDevice.recive();
                }
            }catch (boost::thread_interrupted &e){
                slog::info << "雷达任务关闭" <<radar_path<< slog::endl;
                slamDevice.stop();
                slamDevice.release();
            }
        }
    }
}
